"""  
需求：
流程：
    1.导包；
    2.初始化ROS2客户端；
    3.自定义节点类；
                  
    4.调用spain函数，并传入节点对象；
    5.资源释放。


"""
#1.导包；
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros.transform_broadcaster import TransformBroadcaster
import tf_transformations

#3.自定义节点类；
class DynamticTFBroadcaster(Node):
    def __init__(self):
        super().__init__("dynatic_tf_broadcaster_node")
        self.TFBroadcaster=TransformBroadcaster(self)
        self.timer=self.create_timer(1.0,self.on_timer)
        self.x=0
        self.y=0
        self.z=0
    def on_timer(self):
        ts=TransformStamped()
        ts.header.stamp=self.get_clock().now().to_msg()
        ts.header.frame_id="camera_link"
        ts.child_frame_id="bottle_link"
        self.x+=0.1
        self.y+=0.1
        self.z+=0.1
        ts.transform.translation.x=self.x
        ts.transform.translation.y=self.y
        ts.transform.translation.z=self.z

        qtn=tf_transformations.quaternion_from_euler(0,0,0)
        ts.transform.rotation.x=qtn[0]
        ts.transform.rotation.y=qtn[1]
        ts.transform.rotation.z=qtn[2]
        ts.transform.rotation.w=qtn[3]
        self.TFBroadcaster.sendTransform(ts)
def main():
    #2.初始化ROS2客户端；
    rclpy.init()
    #4.调用spain函数，并传入节点对象；
    rclpy.spin(DynamticTFBroadcaster())
    #5.资源释放。
    rclpy.shutdown()

if __name__=='__main__':
    main() 